//
// File: desired_vel_vg.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 03-Jan-2025 21:39:05
//

// Include Files
#include "desired_vel_vg.h"
#include <algorithm>
#include <cmath>

// Function Definitions
//
// Arguments    : double dg
//                double dob
//                double sig
//                double sio
//                double b
//                double *vR
//                double *vL
// Return Type  : void
//
void desired_vel_vg(double dg, double dob, double sig, double sio, double b,
                    double *vR, double *vL)
{
  static const double dv[101]{0.0,
                              0.02,
                              0.04,
                              0.06,
                              0.08,
                              0.1,
                              0.12,
                              0.14,
                              0.16,
                              0.18,
                              0.2,
                              0.22,
                              0.24,
                              0.26,
                              0.28,
                              0.3,
                              0.32,
                              0.34,
                              0.36,
                              0.38,
                              0.4,
                              0.42,
                              0.44,
                              0.46,
                              0.48,
                              0.5,
                              0.52,
                              0.54,
                              0.56,
                              0.58,
                              0.6,
                              0.62,
                              0.64,
                              0.66,
                              0.68,
                              0.70000000000000007,
                              0.72,
                              0.74,
                              0.76,
                              0.78,
                              0.8,
                              0.82000000000000006,
                              0.84,
                              0.86,
                              0.88,
                              0.9,
                              0.92,
                              0.94000000000000006,
                              0.96,
                              0.98,
                              1.0,
                              1.02,
                              1.04,
                              1.06,
                              1.08,
                              1.1,
                              1.12,
                              1.1400000000000001,
                              1.1600000000000001,
                              1.18,
                              1.2,
                              1.22,
                              1.24,
                              1.26,
                              1.28,
                              1.2999999999999998,
                              1.3199999999999998,
                              1.3399999999999999,
                              1.3599999999999999,
                              1.38,
                              1.4,
                              1.42,
                              1.44,
                              1.46,
                              1.48,
                              1.5,
                              1.52,
                              1.54,
                              1.56,
                              1.58,
                              1.6,
                              1.62,
                              1.6400000000000001,
                              1.66,
                              1.68,
                              1.7,
                              1.72,
                              1.74,
                              1.76,
                              1.78,
                              1.8,
                              1.82,
                              1.84,
                              1.8599999999999999,
                              1.88,
                              1.9,
                              1.92,
                              1.94,
                              1.96,
                              1.98,
                              2.0};
  static const double dv1[101]{0.0,
                               -0.02,
                               -0.04,
                               -0.06,
                               -0.08,
                               -0.1,
                               -0.12,
                               -0.14,
                               -0.16,
                               -0.18,
                               -0.2,
                               -0.22,
                               -0.24,
                               -0.26,
                               -0.28,
                               -0.3,
                               -0.32,
                               -0.34,
                               -0.36,
                               -0.38,
                               -0.4,
                               -0.42,
                               -0.44,
                               -0.46,
                               -0.48,
                               -0.5,
                               -0.52,
                               -0.54,
                               -0.56,
                               -0.58,
                               -0.6,
                               -0.62,
                               -0.64,
                               -0.66,
                               -0.68,
                               -0.70000000000000007,
                               -0.72,
                               -0.74,
                               -0.76,
                               -0.78,
                               -0.8,
                               -0.82000000000000006,
                               -0.84,
                               -0.86,
                               -0.88,
                               -0.9,
                               -0.92,
                               -0.94000000000000006,
                               -0.96,
                               -0.98,
                               -1.0,
                               -1.02,
                               -1.04,
                               -1.06,
                               -1.08,
                               -1.1,
                               -1.12,
                               -1.1400000000000001,
                               -1.1600000000000001,
                               -1.18,
                               -1.2,
                               -1.22,
                               -1.24,
                               -1.26,
                               -1.28,
                               -1.2999999999999998,
                               -1.3199999999999998,
                               -1.3399999999999999,
                               -1.3599999999999999,
                               -1.38,
                               -1.4,
                               -1.42,
                               -1.44,
                               -1.46,
                               -1.48,
                               -1.5,
                               -1.52,
                               -1.54,
                               -1.56,
                               -1.58,
                               -1.6,
                               -1.62,
                               -1.6400000000000001,
                               -1.66,
                               -1.68,
                               -1.7,
                               -1.72,
                               -1.74,
                               -1.76,
                               -1.78,
                               -1.8,
                               -1.82,
                               -1.84,
                               -1.8599999999999999,
                               -1.88,
                               -1.9,
                               -1.92,
                               -1.94,
                               -1.96,
                               -1.98,
                               -2.0};
  double vSect[404];
  double vSectFin_data[404];
  double wgd[101];
  double dv2[6];
  double absxk;
  double pred_x;
  double pred_y;
  double scale;
  int b_i;
  int partialTrueCount;
  int trueCount;
  signed char tmp_data[101];
  // 线速度角速度组
  // 根据目标距离判定线速度
  // 最大最小速度的中间值
  //      if(dg<1)
  //          v=0.2;
  //      else
  //          v=0.4;
  //      end
  // 根据障碍物判定角速度
  if (dob > 1.5) {
    if (sig > 0.0) {
      std::copy(&dv[0], &dv[101], &wgd[0]);
    } else {
      std::copy(&dv1[0], &dv1[101], &wgd[0]);
    }
  } else if (sio > 0.0) {
    std::copy(&dv1[0], &dv1[101], &wgd[0]);
  } else {
    std::copy(&dv[0], &dv[101], &wgd[0]);
  }
  // 参数
  pred_x = dg * std::cos(sig);
  pred_y = dg * std::sin(sig);
  // 时间间隔
  // 预测步长
  //
  //      vSect=[0,0,0];
  trueCount = 0;
  partialTrueCount = 0;
  dv2[3] = 0.0;
  dv2[4] = 0.0;
  dv2[2] = 0.0;
  dv2[5] = 0.1;
  for (int i{0}; i < 101; i++) {
    double po_now[3];
    double t;
    double unnamed_idx_1;
    double y;
    bool b_b;
    //
    po_now[0] = 0.0;
    po_now[1] = 0.0;
    po_now[2] = 0.0;
    b_i = 0;
    vSect[i + 303] = 0.0;
    unnamed_idx_1 = wgd[i];
    for (int j{0}; j < 25; j++) {
      dv2[0] = 0.1 * std::cos(po_now[2]);
      dv2[1] = 0.1 * std::sin(po_now[2]);
      for (int i1{0}; i1 < 3; i1++) {
        po_now[i1] += dv2[i1] * 0.34 + dv2[i1 + 3] * unnamed_idx_1;
      }
    }
    scale = 3.3121686421112381E-170;
    absxk = std::abs(po_now[0]);
    if (absxk > 3.3121686421112381E-170) {
      y = 1.0;
      scale = absxk;
    } else {
      t = absxk / 3.3121686421112381E-170;
      y = t * t;
    }
    absxk = std::abs(po_now[1]);
    if (absxk > scale) {
      t = scale / absxk;
      y = y * t * t + 1.0;
      scale = absxk;
    } else {
      t = absxk / scale;
      y += t * t;
    }
    y = scale * std::sqrt(y);
    absxk = po_now[0] / y;
    if (unnamed_idx_1 < 0.0) {
      absxk = -std::acos(absxk);
    } else {
      absxk = std::acos(absxk);
    }
    //
    if ((dob < 2.0) && (std::abs(sio) < 0.78539816339744828)) {
      if (std::abs(absxk) < 0.35) {
        b_i = 1;
        vSect[i + 303] = 1.0;
      }
    } else if ((dob < 0.8) && (std::abs(absxk) < 0.35)) {
      b_i = 1;
      vSect[i + 303] = 1.0;
    }
    scale = 3.3121686421112381E-170;
    absxk = std::abs(pred_x - po_now[0]);
    if (absxk > 3.3121686421112381E-170) {
      y = 1.0;
      scale = absxk;
    } else {
      t = absxk / 3.3121686421112381E-170;
      y = t * t;
    }
    absxk = std::abs(pred_y - po_now[1]);
    if (absxk > scale) {
      t = scale / absxk;
      y = y * t * t + 1.0;
      scale = absxk;
    } else {
      t = absxk / scale;
      y += t * t;
    }
    vSect[i + 202] = scale * std::sqrt(y);
    vSect[i] = 0.34;
    vSect[i + 101] = unnamed_idx_1;
    b_b = (b_i == 0);
    if (b_b) {
      trueCount++;
      tmp_data[partialTrueCount] = static_cast<signed char>(i + 1);
      partialTrueCount++;
    }
  }
  for (b_i = 0; b_i < 4; b_i++) {
    for (int i1{0}; i1 < trueCount; i1++) {
      vSectFin_data[i1 + trueCount * b_i] =
          vSect[(tmp_data[i1] + 101 * b_i) - 1];
    }
  }
  if (trueCount <= 2) {
    if (trueCount == 1) {
      partialTrueCount = 1;
    } else if (vSectFin_data[trueCount * 2] >
               vSectFin_data[(trueCount + trueCount * 2) - 1]) {
      partialTrueCount = trueCount;
    } else {
      partialTrueCount = 1;
    }
  } else {
    absxk = vSectFin_data[trueCount * 2];
    partialTrueCount = 1;
    for (int i{2}; i <= trueCount; i++) {
      scale = vSectFin_data[(i + trueCount * 2) - 1];
      if (absxk > scale) {
        absxk = scale;
        partialTrueCount = i;
      }
    }
  }
  // 速度输出
  partialTrueCount = tmp_data[partialTrueCount - 1];
  absxk = vSect[partialTrueCount - 1];
  scale = vSect[partialTrueCount + 100] * b;
  *vR = absxk + scale;
  *vL = absxk - scale;
}

//
// File trailer for desired_vel_vg.cpp
//
// [EOF]
//
